#include "gameworld.h"
#include "sbconstants.h"

#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>

#define GRAVITY (1000.f)
#define GRAVITY_PIXEL_SIZE (0.01f)
#define BOUNCE_FRICTION (0.99f)

using namespace std;

// Dot product
float Dot(sf::Vector2f& v1, sf::Vector2f& v2)
{
	return v1.x * v2.x + v1.y * v2.y;
}

// Project vector v onto u
sf::Vector2f Project(sf::Vector2f& v, sf::Vector2f& u)
{
	sf::Vector2f result = u;
	float sqLength = u.x * u.x + u.y * u.y;
	float dot = Dot(u, v);

	result.x *= dot / sqLength;
	result.y *= dot / sqLength;
	return result;
}

GameWorld::GameWorld(bool confirmedStepsOnly, GameworldEventListener* listener) :
	worldTime(0), nextObjectID(1), confirmedStepsOnly(confirmedStepsOnly), gameworldEventListener(listener)
{
}

void GameWorld::SynchronizeFromWorld(GameWorld& other)
{
	// Clear all objects
	for (GameObjectMapIT it = allObjects.begin(); it != allObjects.end(); ++it )
	{
		objectStore.push(it->second);
	}
	allObjects.clear();
	ships.clear();
	planets.clear();
	weapons.clear();

	for (GameObjectMapIT it = other.allObjects.begin(); it != other.allObjects.end(); ++it )
	{
		InsertObject(*it->second);
	}

	// Erase all old bullets
	for (GameObjectListIT it = bullets.begin(); it != bullets.end(); ++it )
	{
		delete *it;
	}
	bullets.clear();

	// Copy from other
	for (GameObjectListIT it = other.bullets.begin(); it != other.bullets.end(); ++it )
	{
		bullets.push_back(new GameObject(**it));
	}

	worldTime = other.worldTime;
	nextObjectID = other.nextObjectID;
}

GameWorld::~GameWorld()
{
}

bool GameWorld::HandleChanges(sf::Uint8 type, sf::Packet& remainder)
{
    sf::Uint8 details = type && MESSAGE::DETAILS_BITS;
	switch(details)
	{
		case MESSAGE::WORLD_UPDATE::CREATE_OBJ:
			{
				GameObject* obj = CreateObject(nextObjectID);
				remainder >> *obj;
			}
			break;

		case MESSAGE::WORLD_UPDATE::DESTROY_OBJ:
			{
				sf::Uint32 objID;
				remainder >> objID;
				DeleteObject(objID);
			}
			break;

		case MESSAGE::WORLD_UPDATE::SHIP_CONTROL:
			{
				sf::Uint32 objID;
				sf::Uint8 controls;
				remainder >> objID >> controls;
				GetObject(objID)->controls = controls;
			}
			break;

		default:
			//std::cerr << "Error identifying world update" << std::endl;
			break;

	}

	return true;
}

bool GameWorld::Step()
{
	++worldTime;

	// Update ships with forces
	for (GameObjectMap::iterator shipIT = ships.begin(); shipIT != ships.end(); ++shipIT)
	{
		GameObject* ship = shipIT->second;
		ship->lifespan += TIME_STEP;
		if (ship->lifespan >= 0.0f)
		{
			for (GameObjectMap::iterator planetIT = planets.begin(); planetIT != planets.end(); ++planetIT)
			{
				GameObject* planet = planetIT->second;
				ApplyGravity(*planet, *ship);
			}
			UpdateShip(*ship);
		}
	}

	// Move all objects
	for (GameObjectMap::iterator objIT = allObjects.begin(); objIT != allObjects.end(); ++objIT)
	{
		GameObject* obj = objIT->second;

		obj->pos.x += (obj->vel.x * TIME_STEP);
		obj->pos.y += (obj->vel.y * TIME_STEP);
		obj->angle += (obj->angVel * TIME_STEP);
	}

	// Move bullets
	for (GameObjectListIT bulletIT = bullets.begin(); bulletIT != bullets.end();)
	{
		GameObject* obj = *bulletIT;
		obj->lifespan -= TIME_STEP;
		//cout << obj->lifespan << std::endl;

		// Erase bullet if it is old.
		if (obj->lifespan <= 0)
		{
			delete obj;
			bulletIT = bullets.erase(bulletIT);
		}
		else
		{
			obj->pos.x += (obj->vel.x * TIME_STEP);
			obj->pos.y += (obj->vel.y * TIME_STEP);
			obj->angle += (obj->angVel * TIME_STEP);
			++bulletIT;
		}
	}

	// Update weapons position (and picked up state and shooting state)
	for (GameObjectMapIT weaponIT = weapons.begin(); weaponIT != weapons.end(); ++weaponIT )
	{
		GameObject& weapon = *(weaponIT->second);
		weapon.weaponCooldown -= TIME_STEP;

		// Weapons without parent are free
		if (weapon.parent == 0)
		{
			bool hit;
			for (GameObjectMapIT shipIT = ships.begin(); shipIT != ships.end(); ++shipIT)
			{
				GameObject& ship = *(shipIT->second);
				ObjectDistSQ(weapon, ship, hit);
				if (hit and ship.lifespan > 0.0f)
				{
					weapon.parent = ship.id;

					// Rotate back to ships 0 degrees
					float x = weapon.pos.x - ship.pos.x;
					float y = weapon.pos.y - ship.pos.y;
					weapon.pos.x = x * cosf(-ship.angle) + y * sinf(-ship.angle);
					weapon.pos.y = -x * sinf(-ship.angle) + y * cosf(-ship.angle);
					weapon.angVel = weapon.vel.x = weapon.vel.y = 0.0f;
					weapon.angle -= ship.angle;
				}
			}

			weapon.vel.x *= 0.95f;
			weapon.vel.y *= 0.95f;
		}
		// Weapon attached to ship... move with ship
		else
		{
			GameObject* ship = GetObject(weapon.parent);
			if (ship)
			{
				// Can and should we fire some projectiles?
				if (weapon.weaponCooldown <= 0.0f && (ship->controls & SHIP_CONTROL::FIRE))
				{
					sf::Vector2f pos = ship->pos;
					pos.x += weapon.pos.x * cosf(ship->angle) + weapon.pos.y * sinf(ship->angle);
					pos.y += -weapon.pos.x * sinf(ship->angle) + weapon.pos.y * cosf(ship->angle);
					pos.x += cosf(weapon.angle + ship->angle) * weapon.size;
					pos.y += -sinf(weapon.angle + ship->angle) * weapon.size;

					{ // TODO: remove creation of bullets from here
						GameObject* b = new GameObject();
						b->id = 0;
						b->pos = pos;
						b->vel.x = ship->vel.x + cos(weapon.angle + ship->angle) * 700.f;
						b->vel.y = ship->vel.y - sin(weapon.angle + ship->angle) * 700.f;
						b->angle = weapon.angle + ship->angle;
						b->size = 1.0f;
						b->lifespan = 3.0f;
						b->angVel = 0.0f;
						bullets.push_back( b );
					}

					// Amount of time until next bullet
					weapon.weaponCooldown = 0.08f;
				}
			}
		}
	}

	// Ships hit planet? or landing...
	for (GameObjectMapIT shipIT = ships.begin(); shipIT != ships.end(); ++shipIT)
	{
		GameObject* ship = shipIT->second;
		for (GameObjectMap::iterator planetIT = planets.begin(); planetIT != planets.end(); ++planetIT)
		{
			GameObject* planet = planetIT->second;
			ResolveObjectPlanetCollision(*ship, *planet);
		}
	}

	// Weapon hit planet?
	for (GameObjectMapIT weaponIT = weapons.begin(); weaponIT != weapons.end(); ++weaponIT)
	{
		GameObject* weapon = weaponIT->second;
		for (GameObjectMap::iterator planetIT = planets.begin(); planetIT != planets.end(); ++planetIT)
		{
			GameObject* planet = planetIT->second;
			ResolveObjectPlanetCollision(*weapon, *planet);
		}
	}

	// Bullet collide with anything?
	for (GameObjectListIT bulletIT = bullets.begin(); bulletIT != bullets.end(); )
	{
		GameObject* bullet = *bulletIT;
		bool hit = false;

		// Test against all planets
		for (GameObjectMapIT planetIT = planets.begin(); hit == false && planetIT != planets.end(); ++planetIT)
		{
			ObjectDistSQ(*bullet, *(planetIT->second), hit);
		}

		// Test against all ships
		for (GameObjectMapIT shipIT = ships.begin(); hit == false && shipIT != ships.end(); ++shipIT)
		{
			ObjectDistSQ(*bullet, *(shipIT->second), hit);
			if (hit and shipIT->second->lifespan >= 2.0f)
			{
				if (confirmedStepsOnly)
				{
					ReleaseWeapons(*(shipIT->second));

					// Only validated moves are allowed to affect players!
					shipIT->second-> angVel += 100; ///< Spin hit ship like crazy.
					shipIT->second->lifespan = -5.0f;
					shipIT->second->pos.x = 100.0f;
					shipIT->second->pos.y = 100.0f;
					shipIT->second->vel.x = 0.0f;
					shipIT->second->vel.y = 0.0f;
					OnShipKilled(*shipIT->second);
				}
			}
		}

		if (hit)
		{
			delete bullet;
			bulletIT = bullets.erase(bulletIT);
		}
		else
		{
			++bulletIT;
		}

	}
	return true;
}

void GameWorld::UpdateShip(GameObject& ship)
{
	// Read and update from ship control
	if (ship.controls & SHIP_CONTROL::LEFT) ship.angVel += 0.3f;
	if (ship.controls & SHIP_CONTROL::RIGHT) ship.angVel -= 0.3f;
	if (ship.controls & SHIP_CONTROL::FORWARD)
	{
		ship.acc.x += cos(ship.angle) * 300.f;
		ship.acc.y += -sin(ship.angle) * 300.f;
	}

	// Update velocity and angle.
	ship.vel.x += ship.acc.x * TIME_STEP;
	ship.vel.y += ship.acc.y * TIME_STEP;
	ship.angVel += ship.angVel * TIME_STEP;

	// Some damping
	ship.angVel *= 0.892f;
	ship.vel.x *= 0.998f;
	ship.vel.y *= 0.998f;
	ship.acc.x = ship.acc.y = 0.0f;
}

void GameWorld::ApplyGravity(GameObject& planet, GameObject& ship)
{
	sf::Vector2f dir(planet.pos.x - ship.pos.x, planet.pos.y - ship.pos.y);
	float sqLength = dir.x * dir.x + dir.y * dir.y;
	float length = sqrtf(sqLength);
	float force = (TIME_STEP * GRAVITY) * (planet.mass / (length * GRAVITY_PIXEL_SIZE));

	dir.x /= length;
	dir.y /= length;
	ship.acc.x += dir.x * force / (ship.mass);
	ship.acc.y += dir.y * force / (ship.mass);
	//cout << "new ship acceleration:" << ship.acc.x << ", " << ship.acc.y << endl;
}

void GameWorld::ReleaseWeapons(GameObject& ship)
{
	// Free all weapons on dying ship
	for (GameObjectMapIT weaponIT = weapons.begin(); weaponIT != weapons.end(); ++weaponIT )
	{
		GameObject& weapon = *(weaponIT->second);
		if (weapon.parent == ship.id)
		{
			weapon.parent = 0;
			sf::Vector2f pos(ship.pos.x, ship.pos.y);
			pos.x += weapon.pos.x * cosf(ship.angle) + weapon.pos.y * sinf(ship.angle);
			pos.y += -weapon.pos.x * sinf(ship.angle) + weapon.pos.y * cosf(ship.angle);
			weapon.pos.x = pos.x;
			weapon.pos.y = pos.y;
			weapon.angle = weapon.angle + ship.angle;
			weapon.angVel = ship.angVel * 2.0f;
			weapon.vel.x = weapon.pos.x - ship.pos.x;
			weapon.vel.y = weapon.pos.y - ship.pos.y;
			float length = sqrtf(weapon.vel.x * weapon.vel.x + weapon.vel.y * weapon.vel.y);
			weapon.vel.x = weapon.vel.x / length * 1000.0f + ship.vel.x;
			weapon.vel.y = weapon.vel.y / length * 1000.0f + ship.vel.y;
		}
	}
}

float GameWorld::ObjectDistSQ(GameObject& obj1, GameObject& obj2, bool& OUTcollides)
{
	float dx = obj1.pos.x - obj2.pos.x;
	float dy = obj1.pos.y - obj2.pos.y;
	float distSQ = dx * dx + dy * dy;
	float minDistSQ = obj1.size + obj2.size;
	minDistSQ *= minDistSQ;
	OUTcollides = minDistSQ > distSQ;
	return distSQ;
}

void GameWorld::ResolveObjectPlanetCollision(GameObject& obj, GameObject& planet)
{
	// Collision
	//sf::Vector2f diff(ship.pos.x - planet.pos.x, ship.pos.y - planet.pos.y);
	//float dist = sqrtf(diff.x * diff.x + diff.y * diff.y);
	bool collision;
	ObjectDistSQ(planet, obj, collision);
	if (collision)
	{
		// Project velocity vector onto ship-planet vector to calculate bounce velocity
		sf::Vector2f dir(planet.pos.x - obj.pos.x, planet.pos.y - obj.pos.y);
		sf::Vector2f bounceVel = Project(obj.vel, dir); // Project Velocity onto ship-to-planet vector
		bounceVel.x *= -1;// * BOUNCE_FRICTION;
		bounceVel.y *= -1;// * BOUNCE_FRICTION;

		obj.vel.x += bounceVel.x;
		obj.vel.y += bounceVel.y;

		// Loose some in the friction between planet and ship
		obj.vel.x *= BOUNCE_FRICTION;
		obj.vel.y *= BOUNCE_FRICTION;

		// TODO: (CHG) - I would like to deactive this
		float lenght = sqrtf(dir.x * dir.x + dir.y * dir.y);
		obj.pos.x = planet.pos.x - dir.x / lenght * (planet.size + obj.size);
		obj.pos.y = planet.pos.y - dir.y / lenght * (planet.size + obj.size);
	}
}

sf::Uint32 GameWorld::CreateObject(sf::Uint8 type, sf::Vector2f pos)
{
	return CreateObject(nextObjectID, type, pos);
}

sf::Uint32 GameWorld::CreateShip(sf::Vector2f pos)
{
	GameObject* obj = CreateObject(nextObjectID);
	obj->type = OBJECT_TYPE::SHIP;
	obj->pos = pos;
	ships[obj->id] = obj;
	obj->vel.x = obj->vel.y = 0;
	obj->angle = 0;
	obj->angVel = 0;
	obj->controls = 0;
	OnObjectCreated(*obj);
	return obj->id;
}

sf::Uint32 GameWorld::CreateObject(sf::Uint32 id, sf::Uint8 type, sf::Vector2f pos)
{
	GameObject* obj = CreateObject(id);

	obj->type = type;
	obj->pos = pos;

	// Add to specialized object map
	switch (type)
	{
		case OBJECT_TYPE::SHIP:
			ships[obj->id] = obj;
			break;

		case OBJECT_TYPE::PLANET:
			planets[obj->id] = obj;
			break;

		case OBJECT_TYPE::BULLET:
			cout << "What the hell are you doing? Creating bullets at line 424 in gameworld.cpp? Moron!" << endl;
			break;

		case OBJECT_TYPE::WEAPON:
			weapons[obj->id] = obj;
			break;

		default:
			//cerr << "Invalid object type being created";
			break;
	}
	OnObjectCreated(*obj);
	return obj->id;
}

bool GameWorld::DeleteObject(sf::Uint32 ID)
{
	GameObject* obj = GetObject(ID);
	if (obj != NULL)
	{
		switch (obj->type)
		{
			case OBJECT_TYPE::SHIP:
				ReleaseWeapons(*obj);
				ships.erase(ID);
				break;

			case OBJECT_TYPE::PLANET:
				planets.erase(ID);
				break;

			case OBJECT_TYPE::WEAPON:
				weapons.erase(ID);
				break;

			case OBJECT_TYPE::BULLET:
				/* bullets.erase(ID); */
				//cout << "What the hell are you doing, destroying bullets in gameworld.cpp line 313!" << endl;
				break;
		}
		allObjects.erase(ID);
		objectStore.push(obj);
		OnObjectDeleted(*obj);
		return true;
	}
	return false;
}

GameObject* GameWorld::GetObject(sf::Uint32 ID)
{
	GameObjectMapIT obj = allObjects.find(ID);
	if (obj == allObjects.end())
		return NULL;
	return obj->second;
}

GameObjectMap GameWorld::GetAllObjects()
{
	return allObjects;
}

bool GameWorld::InsertObject(GameObject& obj)
{
	// Check that the id of this object is unused in the world.
	if (GetObject(obj.id) == NULL)
	{
		sf::Uint32 newObjID = CreateObject(obj.id, obj.type, obj.pos);
		GameObject* newObject = GetObject(newObjID);
		// TODO: This could probably be done with the copy constructor which would be safer
		newObject->parent = obj.parent;
		newObject->acc = obj.acc;
		newObject->angle = obj.angle;
		newObject->angVel = obj.angVel;
		newObject->lifespan = obj.lifespan;
		newObject->controls = obj.controls;

		if (newObject->id != obj.id)
		{
			//cerr << "ID problems " << endl;
			newObject->id = obj.id; ///< Danger will robinson, an existing object on dead stack might have this id already.
		}
		newObject->weaponCooldown = obj.weaponCooldown;
		newObject->mass = obj.mass;
		newObject->size = obj.size;
		newObject->vel = obj.vel;

		if (nextObjectID <= obj.id)
		{
			++nextObjectID;
		}

		OnObjectCreated(*newObject);

		return true;
	}
	else
	{
		//cerr << "Error inserting object, the object ID is already used" << endl;
		return false;
	}
}

void GameWorld::UpdateShipControls(GameObject& object, sf::Uint8 newControls)
{
	object.controls = newControls;
	OnShipControlChanged(object);
}


// Private methods
///////////////////////////////////////////////////
GameObject* GameWorld::CreateObject(sf::Uint32 objID)
{
	GameObject* obj = NULL;
	if (objectStore.empty())
	{
		obj = new GameObject();
	}
	else
	{
		obj = objectStore.top();
		objectStore.pop();
	}

	// Add to general object map
	obj->id = objID;
	allObjects[objID]=obj;

	if (objID >= nextObjectID)
	{
		++nextObjectID;
	}
	//std::cout << "Created object: " << obj->id << std::endl;
	return obj;
}

// Event propegation
void GameWorld::OnObjectCreated(GameObject& object)
{
	if (gameworldEventListener != NULL)
	{
		gameworldEventListener->ObjectCreated(object);
	}
}
void GameWorld::OnObjectDeleted(GameObject& object)
{
	if (gameworldEventListener != NULL)
	{
		gameworldEventListener->ObjectDeleted(object);
	}
}
void GameWorld::OnShipKilled(GameObject& object)
{
	if (gameworldEventListener != NULL)
	{
		gameworldEventListener->ShipKilled(object);
	}
}

void GameWorld::OnShipControlChanged(GameObject& object)
{
	if (gameworldEventListener != NULL)
	{
		gameworldEventListener->ShipControlChanged(object);
	}
}
